#include "Timers.h"
#include "avrlibdefs.h"
#include "globals.h"
#ifdef DEBUGTIMER

#endif
#include <avr/interrupt.h>

typedef void (*voidFuncPtr)(void);
volatile static voidFuncPtr timer_int_func[(enum _eTimerInterrupts)eTimerMax];

typedef struct _typTimerInfo {
	unsigned short				TopValue;
	unsigned char				BitSize;
	enum _eTimerDividerScale	eDividerScale;	
	unsigned char				Divider;
	unsigned char				Mode;
} _TimerInfo;

//--------------------------------------------------------------------------------------------
// Local Variables
struct _typTimerInfo	typTimerInfo[] =
{
#ifdef atmega1280
	{0xFF	,0x8	,eTmrNormal	,TIMER_CLK_STOP,	TIMER_NORMAL},		
	{0xFFFF	,0x10	,eTmrNormal	,TIMER_CLK_STOP,	TIMER_NORMAL},	// Thread Scheduler (trial)
	{0xFF	,0x8	,eTmrRTC	,TIMER_CLK_STOP,	TIMER_NORMAL},	
	{0xFFFF	,0x10	,eTmrNormal	,TIMER_CLK_STOP,	TIMER_NORMAL},	// Aux1-2
	{0xFFFF	,0x10	,eTmrNormal	,TIMER_CLK_STOP,	TIMER_NORMAL},	// Aux3-4
	{103	,0x10	,eTmrNormal	,TIMER_CLK_DIV8,	TIMER_PWM}		// Consult Uart Clock
#endif
#ifdef atmega128
	{ 0xFF	,0x8	,eTmrRTC	, TIMER_CLK_STOP	,TIMER_NORMAL },
	{ 0xFFFF,0x10	,eTmrNormal	, TIMER_CLK_STOP	,TIMER_NORMAL },
	{ 0xFF	,0x8	,eTmrNormal	, TIMER_CLK_STOP	,TIMER_NORMAL },
	{ 0xFFFF,0x10	,eTmrNormal	, TIMER_CLK_STOP	,TIMER_NORMAL }
#endif
} ;


//--------------------------------------------------------------------------------------------
// Private Functions


//--------------------------------------------------------------------------------------------
// Public Functions
// Initialise a timer

void timers_initialise	(void)
{
	enum _eTimerInterrupts i;
	for(i=0;i<eTimerMax;i++)
		timer_int_func[i] = 0x00;

	timer_initialise(0);
	timer_initialise(1);
	timer_initialise(2);
	timer_initialise(3);
}

void	timer_initialise			(unsigned char num)
{
	struct _typTimerInfo *ptypTimer = &typTimerInfo[num];

	switch (num)
	{
	case 0:
#ifdef atmega128
		switch (ptypTimer->Mode)
		{
		case TIMER_NORMAL:
			//cbi(TCCR0,WGM02);
			cbi(TCCR0,WGM01);
			cbi(TCCR0,WGM00);
			break;
		case TIMER_CTC:
			//cbi(TCCR0,WGM02);
			sbi(TCCR0,WGM01);
			cbi(TCCR0,WGM00);
			break;
		case TIMER_PWM:
			//cbi(TCCR0,WGM02);
			sbi(TCCR0, WGM01);
			sbi(TCCR0, WGM00);
			break;
		case TIMER_PWMFC:
			//cbi(TCCR0,WGM02);
			cbi(TCCR0, WGM01);
			sbi(TCCR0, WGM00);
			break;
		}

		// Set Divider
		outb(TCCR0, (inb(TCCR0) & ~TIMER_PRESCALE_MASK) | ptypTimer->Divider);
#else
	switch (ptypTimer->Mode)
		{
		case TIMER_NORMAL:
			cbi(TCCR0B,WGM02);
			cbi(TCCR0A,WGM01);
			cbi(TCCR0A,WGM00);
			break;
		case TIMER_CTC:
			cbi(TCCR0B,WGM02);
			sbi(TCCR0A,WGM01);
			cbi(TCCR0A,WGM00);
			break;
		case TIMER_PWM:
			cbi(TCCR0B,WGM02);
			sbi(TCCR0A, WGM01);
			sbi(TCCR0A, WGM00);
			break;
		case TIMER_PWMFC:
			cbi(TCCR0B,WGM02);
			cbi(TCCR0A, WGM01);
			sbi(TCCR0A, WGM00);
			break;
		}
		
		// Set Divider
		outb(TCCR0B, ((inb(TCCR0B) & ~0x07) | ptypTimer->Divider));

		// Reset OCRo
		if (ptypTimer->Mode == TIMER_CTC)
			outb(OCR0A, ptypTimer->TopValue);
		else
			outb(OCR0A, 0);

		outb(OCR0B, 0);
#endif

		// Reset TCNT0
		outb(TCNT0, 0);	

		break;
	case 1:

		switch (ptypTimer->Mode)
		{
		case TIMER_NORMAL:
			cbi(TCCR1A,WGM10);
			cbi(TCCR1A,WGM11);
			cbi(TCCR1B,WGM12);
			cbi(TCCR1B,WGM13);
			break;
		case TIMER_CTC:
			cbi(TCCR1A,WGM10);
			cbi(TCCR1A,WGM11);
			sbi(TCCR1B,WGM12);
			sbi(TCCR1B,WGM13);
			break;
		case TIMER_PWM:
			cbi(TCCR1A,WGM10);
			sbi(TCCR1A,WGM11);
			sbi(TCCR1B,WGM12);
			sbi(TCCR1B,WGM13);
			break;
		case TIMER_PWMFC:
			cbi(TCCR1A,WGM10);
			cbi(TCCR1A,WGM11);
			cbi(TCCR1B,WGM12);
			sbi(TCCR1B,WGM13);
			break;
		}

		// Set Divider
		outb(TCCR1B, (inb(TCCR1B) & ~TIMER_PRESCALE_MASK) | ptypTimer->Divider);

		// Reset TCNT3
		outb(TCNT1H, 0);
		outb(TCNT1L, 0);

		// set top count value
		ICR1H = (unsigned char)(ptypTimer->TopValue>>8);
		ICR1L = (unsigned char)ptypTimer->TopValue;

		// clear output compare value A
		outb(OCR1AH, 0);
		outb(OCR1AL, 0);
		// clear output compare value B
		outb(OCR1BH, 0);
		outb(OCR1BL, 0);
		// clear output compare value C
		outb(OCR1CH, 0);
		outb(OCR1CL, 0);

		break;
	case 2:

#ifdef atmega128
		switch (ptypTimer->Mode)
		{
		case TIMER_NORMAL:
			cbi(TCCR2,WGM21);
			cbi(TCCR2,WGM20);
			break;
		case TIMER_CTC:
			sbi(TCCR2,WGM21);
			cbi(TCCR2,WGM20);
			break;
		case TIMER_PWM:
			sbi(TCCR2, WGM21);
			sbi(TCCR2, WGM20);
			break;
		case TIMER_PWMFC:
			cbi(TCCR2, WGM21);
			sbi(TCCR2, WGM20);
			break;
		}

		// Set Divider
		outb(TCCR2, (inb(TCCR2) & ~TIMER_PRESCALE_MASK) | ptypTimer->Divider);

		// Reset TCNT0
		outb(TCNT2, 0);	

		// Set Top Value
		outb(OCR2, ptypTimer->TopValue);
#endif
#ifdef atmega1280
		switch (ptypTimer->Mode)
		{
		case TIMER_NORMAL:
			cbi(TCCR2B,WGM22);
			cbi(TCCR2A,WGM21);
			cbi(TCCR2A,WGM20);
			break;
		case TIMER_CTC:
			cbi(TCCR2B,WGM22);
			sbi(TCCR2A,WGM21);
			cbi(TCCR2A,WGM20);
			break;
		case TIMER_PWM:
			cbi(TCCR2B,WGM22);
			sbi(TCCR2A, WGM21);
			sbi(TCCR2A, WGM20);
			break;
		case TIMER_PWMFC:
			cbi(TCCR2B,WGM22);
			cbi(TCCR2A, WGM21);
			sbi(TCCR2A, WGM20);
			break;
		}

		// Reset OCR2n
		if (ptypTimer->Mode == TIMER_CTC)
			outb(OCR2A, ptypTimer->TopValue);
		else
			outb(OCR2A, 0);

		outb(OCR2B, 0);
		
		// Set Divider
		outb(TCCR2B, (inb(TCCR2B) & ~0x07) | ptypTimer->Divider);
#endif

		// Reset TCNT2
		outb(TCNT2, 0);	

		break;

	case 3:
		switch (ptypTimer->Mode)
		{
		case TIMER_NORMAL:
			cbi(TCCR3A,WGM30);
			cbi(TCCR3A,WGM31);
			cbi(TCCR3B,WGM32);
			cbi(TCCR3B,WGM33);
			break;
		case TIMER_CTC:
			cbi(TCCR3A,WGM30);
			cbi(TCCR3A,WGM31);
			sbi(TCCR3B,WGM32);
			sbi(TCCR3B,WGM33);
			break;
		case TIMER_PWM:
			cbi(TCCR3A,WGM30);
			sbi(TCCR3A,WGM31);
			sbi(TCCR3B,WGM32);
			sbi(TCCR3B,WGM33);
			break;
		case TIMER_PWMFC:
			cbi(TCCR3A,WGM30);
			cbi(TCCR3A,WGM31);
			cbi(TCCR3B,WGM32);
			sbi(TCCR3B,WGM33);
			break;
		}

		// Set Divider
		outb(TCCR3B, (inb(TCCR3B) & ~TIMER_PRESCALE_MASK) | ptypTimer->Divider);

		// Reset TCNT3
		outb(TCNT3H, 0);
		outb(TCNT3L, 0);

		// set top count value
		ICR3H = (unsigned char)(ptypTimer->TopValue>>8);
		ICR3L = (unsigned char)ptypTimer->TopValue;

		// clear output compare value A
		outb(OCR3AH, 0);
		outb(OCR3AL, 0);
		// clear output compare value B
		outb(OCR3BH, 0);
		outb(OCR3BL, 0);
		// clear output compare value C
		outb(OCR3CH, 0);
		outb(OCR3CL, 0);

		break;
#ifdef atmega1280
	case 4:

		switch (ptypTimer->Mode)
		{
		case TIMER_NORMAL:
			cbi(TCCR4A,WGM40);
			cbi(TCCR4A,WGM41);
			cbi(TCCR4B,WGM42);
			cbi(TCCR4B,WGM43);
			break;
		case TIMER_CTC:
			cbi(TCCR4A,WGM40);
			cbi(TCCR4A,WGM41);
			sbi(TCCR4B,WGM42);
			sbi(TCCR4B,WGM43);
			break;
		case TIMER_PWM:
			cbi(TCCR4A,WGM40);
			sbi(TCCR4A,WGM41);
			sbi(TCCR4B,WGM42);
			sbi(TCCR4B,WGM43);
			break;
		case TIMER_PWMFC:
			cbi(TCCR4A,WGM40);
			cbi(TCCR4A,WGM41);
			cbi(TCCR4B,WGM42);
			sbi(TCCR4B,WGM43);
			break;
		}

		// Set Divider
		outb(TCCR4B, (inb(TCCR4B) & ~TIMER_PRESCALE_MASK) | ptypTimer->Divider);

		// Reset TCNT3
		outb(TCNT4H, 0);
		outb(TCNT4L, 0);

		// set top count value
		ICR4H = (uint8_t)(ptypTimer->TopValue>>8);
		ICR4L = (uint8_t)ptypTimer->TopValue;

		// clear output compare value A
		outb(OCR4AH, 0);
		outb(OCR4AL, 0);
		// clear output compare value B
		outb(OCR4BH, 0);
		outb(OCR4BL, 0);
		// clear output compare value C
		outb(OCR4CH, 0);
		outb(OCR4CL, 0);

		break;

	case 5:

		switch (ptypTimer->Mode)
		{
		case TIMER_NORMAL:
			cbi(TCCR5A,WGM50);
			cbi(TCCR5A,WGM51);
			cbi(TCCR5B,WGM52);
			cbi(TCCR5B,WGM53);
			break;
		case TIMER_CTC:
			cbi(TCCR5A,WGM50);
			cbi(TCCR5A,WGM51);
			sbi(TCCR5B,WGM52);
			sbi(TCCR5B,WGM53);
			break;
		case TIMER_PWM:
			cbi(TCCR5A,WGM50);
			sbi(TCCR5A,WGM51);
			sbi(TCCR5B,WGM52);
			sbi(TCCR5B,WGM53);
			break;
		case TIMER_PWMFC:
			cbi(TCCR5A,WGM50);
			cbi(TCCR5A,WGM51);
			cbi(TCCR5B,WGM52);
			sbi(TCCR5B,WGM53);
			break;
		}

		// Set Divider
		outb(TCCR5B, (inb(TCCR5B) & ~TIMER_PRESCALE_MASK) | ptypTimer->Divider);

		// Reset TCNT3
		outb(TCNT5H, 0);
		outb(TCNT5L, 0);

		// set top count value
		ICR5H = (uint8_t)(ptypTimer->TopValue>>8);
		ICR5L = (uint8_t)ptypTimer->TopValue;

		// clear output compare value A
		outb(OCR5AH, 0);
		outb(OCR5AL, 0);
		// clear output compare value B
		outb(OCR5BH, 0);
		outb(OCR5BL, 0);
		// clear output compare value C
		outb(OCR5CH, 0);
		outb(OCR5CL, 0);

		break;
#endif
	}

}

unsigned short	timer_initialise_mode_exact	(unsigned char num, unsigned char div, unsigned short topVal, unsigned char mode)
{
	// Get the structe for the info
	struct _typTimerInfo *ptypTimer = &typTimerInfo[num];
	unsigned long	nMaxTopCnt=topVal;

	ptypTimer->Mode = mode;

	if (mode == TIMER_NORMAL)
	{
		ptypTimer->Divider = TIMER_CLK_STOP;
	}
	else
	{
		ptypTimer->Divider = div;
		
		// Perform basic check for top value
		if (ptypTimer->BitSize == 0x08)
			if (nMaxTopCnt > 0xFF)
				ptypTimer->TopValue = 0xFF;
			else
				ptypTimer->TopValue = nMaxTopCnt;
		else
			if (nMaxTopCnt > 0xFFFF)
				ptypTimer->TopValue = 0xFFFF;
			else
				ptypTimer->TopValue = nMaxTopCnt;
	}

	// Always call Initialise after a mode change
	timer_initialise (num);

	return ptypTimer->TopValue;
}

// Initialise a timer pwm frequency
unsigned short	timer_initialise_mode		(unsigned char num, float freq, unsigned char mode)
{
	// Get the structe for the info
	struct _typTimerInfo *ptypTimer = &typTimerInfo[num];
	unsigned long	nMaxTopCnt;
	unsigned char	nMult=1;

	ptypTimer->Mode = mode;

	if (mode == TIMER_NORMAL)
	{
		ptypTimer->Divider = TIMER_CLK_STOP;
	}
	else
	{
		if (ptypTimer->BitSize == 0x08)
			nMaxTopCnt = 0x100;
		else
			nMaxTopCnt = 0x10000;

		if (mode == TIMER_PWM || mode == TIMER_CTC)
		{
			nMult = 1;	// if mode PWM or CTC, calculate frequency with normal value
		}
		else if (mode == TIMER_PWMFC)
		{
			nMult = 2; 	// if mode PWM PC calculate frequency with double value
		}

		if (ptypTimer->eDividerScale == eTmrRTC)
		{
			if ( freq > DIV1MAX(nMaxTopCnt*nMult) ) {
				ptypTimer->Divider = TIMERRTC_CLK_DIV1;
				nMaxTopCnt = ((float)F_CPU/(freq*(float)nMult))-1;
			} else if ( freq > DIV8MAX(nMaxTopCnt*nMult) ) {
				ptypTimer->Divider = TIMERRTC_CLK_DIV8;
				nMaxTopCnt = ((float)F_CPU/((float)8*freq*(float)nMult))-1;
			} else if ( freq > DIV32MAX(nMaxTopCnt*nMult) ) {
				ptypTimer->Divider = TIMERRTC_CLK_DIV32;
				nMaxTopCnt = ((float)F_CPU/((float)32*freq*(float)nMult))-1;
			} else if ( freq > DIV64MAX(nMaxTopCnt*nMult) ) {
				ptypTimer->Divider = TIMERRTC_CLK_DIV64;
				nMaxTopCnt = ((float)F_CPU/((float)64*freq*(float)nMult))-1;
			} else if ( freq > DIV128MAX(nMaxTopCnt*nMult) ) {
				ptypTimer->Divider = TIMERRTC_CLK_DIV128;
				nMaxTopCnt = ((float)F_CPU/((float)128*freq*(float)nMult))-1;
			} else if ( freq > DIV256MAX(nMaxTopCnt*nMult) ) {
				ptypTimer->Divider = TIMERRTC_CLK_DIV256;
				nMaxTopCnt = ((float)F_CPU/((float)256*freq*(float)nMult))-1;
			} else {
				ptypTimer->Divider = TIMERRTC_CLK_DIV1024;
				nMaxTopCnt = ((float)F_CPU/((float)1024*freq*(float)nMult))-1;
			}
		}
		else if (ptypTimer->eDividerScale == eTmrNormal)
		{
			if ( freq > DIV1MAX(nMaxTopCnt*nMult) ) {
				ptypTimer->Divider = TIMER_CLK_DIV1;
				nMaxTopCnt = ((float)F_CPU/(freq*(float)nMult))-1;
			} else if ( freq > DIV8MAX(nMaxTopCnt*nMult) ) {
				ptypTimer->Divider = TIMER_CLK_DIV8;
				nMaxTopCnt = ((float)F_CPU/((float)8*freq*(float)nMult))-1;
			} else if ( freq > DIV64MAX(nMaxTopCnt*nMult) ) {
				ptypTimer->Divider = TIMER_CLK_DIV64;
				nMaxTopCnt = ((float)F_CPU/((float)64*freq*(float)nMult))-1;
			} else if ( freq > DIV256MAX(nMaxTopCnt*nMult) ) {
				ptypTimer->Divider = TIMER_CLK_DIV256;
				nMaxTopCnt = ((float)F_CPU/((float)256*freq*(float)nMult))-1;
			} else {
				ptypTimer->Divider = TIMER_CLK_DIV1024;
				nMaxTopCnt = ((float)F_CPU/((float)1024*freq*(float)nMult))-1;
			}
		}

		// Perform basic check for top value
		if (ptypTimer->BitSize == 0x08)
			if (nMaxTopCnt > 0xFF)
				ptypTimer->TopValue = 0xFF;
			else
				ptypTimer->TopValue = nMaxTopCnt;
		else
			if (nMaxTopCnt > 0xFFFF)
				ptypTimer->TopValue = 0xFFFF;
			else
				ptypTimer->TopValue = nMaxTopCnt;
	}


	// Always call Initialise after a mode change
	timer_initialise (num);

	return ptypTimer->TopValue;
}

#define outb_reg(adr,bit,msk)	outb(adr,(inb(adr) & ~BV(bit)) | (BV(bit) & msk))

// Attach a function to a timer overflow
void	timer_attach				(enum _eTimerInterrupts Interrupt, void (*userFunc)(void))
{
	unsigned char	mask = userFunc != 0x00 ? 0xFF : 0x00;

	timer_int_func[Interrupt] = userFunc;

	switch (Interrupt)
	{
#ifdef atmega1280
		case eTimer0_CompareA:
			outb_reg(TIMSK0, OCIE0A, mask);
			break;
		case eTimer0_CompareB:
			outb_reg(TIMSK0, OCIE0B, mask);
			break;
		case eTimer0_Overflow:
			outb_reg(TIMSK0, TOIE0, mask);
			break;
		case eTimer1_Capture:
			outb_reg(TIMSK1, ICIE1, mask);
			break;
		case eTimer1_CompareA:
			outb_reg(TIMSK1, OCIE1A, mask);
			break;
		case eTimer1_CompareB:
			outb_reg(TIMSK1, OCIE1B, mask);
			break;
		case eTimer1_CompareC:
			outb_reg(TIMSK1, OCIE1C, mask);
			break;
		case eTimer1_Overflow:
			outb_reg(TIMSK1, TOIE1, mask);
			break;
		case eTimer2_CompareA:
			outb_reg(TIMSK2, OCIE2A, mask);
			break;
		case eTimer2_CompareB:
			outb_reg(TIMSK2, OCIE2B, mask);
			break;
		case eTimer2_Overflow:
			outb_reg(TIMSK2, TOIE2, mask);
			break;
		case eTimer3_Capture:
			outb_reg(TIMSK3, ICIE3, mask);
			break;
		case eTimer3_CompareA:
			outb_reg(TIMSK3, OCIE3A, mask);
			break;
		case eTimer3_CompareB:
			outb_reg(TIMSK3, OCIE3B, mask);
			break;
		case eTimer3_CompareC:
			outb_reg(TIMSK3, OCIE3C, mask);
			break;
		case eTimer3_Overflow:
			outb_reg(TIMSK3, TOIE3, mask);
			break;
		case eTimer4_Capture:
			outb_reg(TIMSK4, ICIE4, mask);
			break;
		case eTimer4_CompareA:
			outb_reg(TIMSK4, OCIE4A, mask);
			break;
		case eTimer4_CompareB:
			outb_reg(TIMSK4, OCIE4B, mask);
			break;
		case eTimer4_CompareC:
			outb_reg(TIMSK4, OCIE4C, mask);
			break;
		case eTimer4_Overflow:
			outb_reg(TIMSK4, TOIE4, mask);
			break;
		case eTimer5_Capture:
			outb_reg(TIMSK5, ICIE5, mask);
			break;
		case eTimer5_CompareA:
			outb_reg(TIMSK5, OCIE5A, mask);
			break;
		case eTimer5_CompareB:
			outb_reg(TIMSK5, OCIE5B, mask);
			break;
		case eTimer5_CompareC:
			outb_reg(TIMSK5, OCIE5C, mask);
			break;
		case eTimer5_Overflow:
			outb_reg(TIMSK5, TOIE5, mask);
			break;
#endif
#ifdef atmega128
		case eTimer0_Compare:
			outb_reg(TIMSK, OCIE0, mask);
			break;
		case eTimer0_Overflow:
			outb_reg(TIMSK, TOIE0, mask);
			break;
		case eTimer1_Capture:
			outb_reg(TIMSK, TICIE1, mask);
			break;
		case eTimer1_CompareA:
			outb_reg(TIMSK, OCIE1A, mask);
			break;
		case eTimer1_CompareB:
			outb_reg(TIMSK, OCIE1B, mask);
			break;
		case eTimer1_CompareC:
			outb_reg(ETIMSK, OCIE1C, mask);
			break;
		case eTimer1_Overflow:
			outb_reg(TIMSK, TOIE1, mask);
			break;
		case eTimer2_Compare:
			outb_reg(TIMSK, OCIE2, mask);
			break;
		case eTimer2_Overflow:
			outb_reg(TIMSK, TOIE2, mask);
			break;
		case eTimer3_Capture:
			outb_reg(ETIMSK, TICIE3, mask);
			break;
		case eTimer3_CompareA:
			outb_reg(ETIMSK, OCIE1A, mask);
			break;
		case eTimer3_CompareB:
			outb_reg(ETIMSK, OCIE1B, mask);
			break;
		case eTimer3_CompareC:
			outb_reg(ETIMSK, OCIE1C, mask);
			break;
		case eTimer3_Overflow:
			outb_reg(ETIMSK, TOIE3, mask);
			break;
#endif
		case eTimerMax:
		default:
			break;
		}
}
// Turn the PWM channel for timer on or off
void	timer_pwm_set				(unsigned char num, unsigned char chan, unsigned char state)
{

}
// Set the current value of the PWM output
void	timer_pwm_set_value			(unsigned char num, unsigned char chan, float nValue)
{

}


// Get the status of a timer
unsigned char	timer_status				(unsigned char num)
{
	return typTimerInfo[num].Divider;
}

inline unsigned short timer_X_pwm_value (unsigned char Num, float Value)
{
	if (Value > 1)
		return typTimerInfo[Num].TopValue;
	else if (Value < 0)
		return 0;
	else
		return typTimerInfo[Num].TopValue * Value;
}

#ifdef atmega128

void timer_0_pwm_set	(unsigned char state)
{
	switch (state)
	{
	case 2:
		cbi(TCCR0, COM01);
		sbi(TCCR0, COM00);
	case 1:
		sbi(TCCR0, COM01);
		cbi(TCCR0, COM00);
	case 0:
		cbi(TCCR0, COM01);
		cbi(TCCR0, COM00);
	}
}

void timer_0_pwm_set_value (float nValue)
{
	unsigned char	pwmDuty = timer_X_pwm_value(0,nValue);
	outb(OCR0, (pwmDuty));		// set the high 8bits of OCR4A
}

void timer_2_pwm_set	(unsigned char state)
{
	switch (state)
	{
	case 2:
		cbi(TCCR2, COM21);
		sbi(TCCR2, COM20);
	case 1:
		sbi(TCCR2, COM21);
		cbi(TCCR2, COM20);
	case 0:
		cbi(TCCR2, COM21);
		cbi(TCCR2, COM20);
	}
}

void timer_2_pwm_set_value (float nValue)
{
	unsigned char	pwmDuty = timer_X_pwm_value(2,nValue);
	outb(OCR2, (pwmDuty));		// set the high 8bits of OCR4A
}


#endif
#ifdef atmega1280

void timer_0_pwm_A_set	(unsigned char state)
{
	if (state)
	{
		sbi(TCCR0A, COM0A1);
		cbi(TCCR0A, COM0A0);
	}
	else
	{
		cbi(TCCR0A, COM0A1);
		cbi(TCCR0A, COM0A0);
	}
}

void timer_0_pwm_B_set	(unsigned char state)
{
	if (state)
	{
		sbi(TCCR0A, COM0B1);
		cbi(TCCR0A, COM0B0);
	}
	else
	{
		cbi(TCCR0A, COM0B1);
		cbi(TCCR0A, COM0B0);
	}
}

void timer_0_pwm_A_set_value (float nValue)
{
	unsigned char	pwmDuty = timer_X_pwm_value(0,nValue);
	outb(OCR0A, (pwmDuty));		// set the high 8bits of OCR4A
}

void timer_0_pwm_B_set_value (float nValue)
{
	unsigned char	pwmDuty = timer_X_pwm_value(0,nValue);
	outb(OCR0B, (pwmDuty));		// set the high 8bits of OCR4A
}

void timer_2_pwm_A_set	(unsigned char state)
{
	if (state)
	{
		sbi(TCCR2A, COM2A1);
		cbi(TCCR2A, COM2A0);
	}
	else
	{
		cbi(TCCR2A, COM2A1);
		cbi(TCCR2A, COM2A0);
	}
}

void timer_2_pwm_B_set	(unsigned char state)
{
	if (state)
	{
		sbi(TCCR2A, COM2B1);
		cbi(TCCR2A, COM2B0);
	}
	else
	{
		cbi(TCCR2A, COM2B1);
		cbi(TCCR2A, COM2B0);
	}
}

void timer_2_pwm_A_set_value (float nValue)
{
	unsigned char	pwmDuty = timer_X_pwm_value(2,nValue);
	outb(OCR2A, (pwmDuty));		// set the high 8bits of OCR4A
}

void timer_2_pwm_B_set_value (float nValue)
{
	unsigned char	pwmDuty = timer_X_pwm_value(2,nValue);
	outb(OCR2B, (pwmDuty));		// set the high 8bits of OCR4A
}


void timer_4_pwm_A_set	(unsigned char state)
{
	if (state == 2)
	{
		sbi(TCCR4A,COM4A1);
		sbi(TCCR4A,COM4A0);
	}
	else if (state)
	{
		sbi(TCCR4A,COM4A1);
		cbi(TCCR4A,COM4A0);
	}
	else
	{
		cbi(TCCR4A,COM4A1);
		cbi(TCCR4A,COM4A0);
	}
}
void timer_4_pwm_B_set	(unsigned char state)
{
	if (state == 2)
	{
		sbi(TCCR4A,COM4B1);
		sbi(TCCR4A,COM4B0);
	}
	else if (state)
	{
		sbi(TCCR4A,COM4B1);
		cbi(TCCR4A,COM4B0);
	}
	else
	{
		cbi(TCCR4A,COM4B1);
		cbi(TCCR4A,COM4B0);
	}
}
void timer_4_pwm_C_set	(unsigned char state)
{
	if (state==2)
	{
		sbi(TCCR4A,COM4C1);
		sbi(TCCR4A,COM4C0);
	}
	else if (state)
	{
		sbi(TCCR4A,COM4C1);
		cbi(TCCR4A,COM4C0);
	}
	else
	{
		cbi(TCCR4A,COM4C1);
		cbi(TCCR4A,COM4C0);
	}
}

void timer_4_pwm_A_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(4,nValue);
	outb(OCR4AH, (pwmDuty>>8));		// set the high 8bits of OCR4A
	outb(OCR4AL, (pwmDuty&0x00FF));	// set the low 8bits of OCR4A
}
void timer_4_pwm_B_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(4,nValue);
	outb(OCR4BH, (pwmDuty>>8));		// set the high 8bits of OCR4B
	outb(OCR4BL, (pwmDuty&0x00FF));	// set the low 8bits of OCR4B
}
void timer_4_pwm_C_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(4,nValue);
	outb(OCR4CH, (pwmDuty>>8));		// set the high 8bits of OCR4C
	outb(OCR4CL, (pwmDuty&0x00FF));	// set the low 8bits of OCR4C
}

void timer_5_pwm_A_set	(unsigned char state)
{
	if (state)
	{
		sbi(TCCR5A,COM5A1);
		sbi(TCCR5A,COM5A0);
	}
	else
	{
		cbi(TCCR5A,COM5A1);
		cbi(TCCR5A,COM5A0);
	}
}
void timer_5_pwm_B_set	(unsigned char state)
{
	if (state)
	{
		sbi(TCCR5A,COM5B1);
		cbi(TCCR5A,COM5B0);
	}
	else
	{
		cbi(TCCR5A,COM5B1);
		cbi(TCCR5A,COM5B0);
	}
}
void timer_5_pwm_C_set	(unsigned char state)
{
	if (state)
	{
		sbi(TCCR5A,COM5C1);
		cbi(TCCR5A,COM5C0);
	}
	else
	{
		cbi(TCCR5A,COM5C1);
		cbi(TCCR5A,COM5C0);
	}
}

void timer_5_pwm_A_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(5,nValue);
	outb(OCR5AH, (pwmDuty>>8));		// set the high 8bits of OCR5A
	outb(OCR5AL, (pwmDuty&0x00FF));	// set the low 8bits of OCR5A
}
void timer_5_pwm_B_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(5,nValue);
	outb(OCR5BH, (pwmDuty>>8));		// set the high 8bits of OCR5B
	outb(OCR5BL, (pwmDuty&0x00FF));	// set the low 8bits of OCR5B
}
void timer_5_pwm_C_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(5,nValue);
	outb(OCR5CH, (pwmDuty>>8));		// set the high 8bits of OCR5C
	outb(OCR5CL, (pwmDuty&0x00FF));	// set the low 8bits of OCR5C
}

#endif

void timer_1_pwm_A_set	(unsigned char state)
{
	if (state == 2)
	{
		sbi(TCCR1A,COM1A1);
		sbi(TCCR1A,COM1A0);
	}
	else if (state)
	{
		sbi(TCCR1A,COM1A1);
		cbi(TCCR1A,COM1A0);
	}
	else
	{
		cbi(TCCR1A,COM1A1);
		cbi(TCCR1A,COM1A0);
	}
}
void timer_1_pwm_B_set	(unsigned char state)
{
	if (state == 2)
	{
		sbi(TCCR1A,COM1B1);
		sbi(TCCR1A,COM1B0);
	}
	else if (state)
	{
		sbi(TCCR1A,COM1B1);
		cbi(TCCR1A,COM1B0);
	}
	else
	{
		cbi(TCCR1A,COM1B1);
		cbi(TCCR1A,COM1B0);
	}
}
void timer_1_pwm_C_set	(unsigned char state)
{
	if (state == 2)
	{
		sbi(TCCR1A,COM1C1);
		sbi(TCCR1A,COM1C0);
	}
	else if (state)
	{
		sbi(TCCR1A,COM1C1);
		cbi(TCCR1A,COM1C0);
	}
	else
	{
		cbi(TCCR1A,COM1C1);
		cbi(TCCR1A,COM1C0);
	}
}

void timer_1_pwm_A_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(1,nValue);
	outb(OCR1AH, (pwmDuty>>8));		// set the high 8bits of OCR1A
	outb(OCR1AL, (pwmDuty&0x00FF));	// set the low 8bits of OCR1A
}
void timer_1_pwm_B_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(1,nValue);
	outb(OCR1BH, (pwmDuty>>8));		// set the high 8bits of OCR1B
	outb(OCR1BL, (pwmDuty&0x00FF));	// set the low 8bits of OCR1B
}
void timer_1_pwm_C_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(1,nValue);
	outb(OCR1CH, (pwmDuty>>8));		// set the high 8bits of OCR1C
	outb(OCR1CL, (pwmDuty&0x00FF));	// set the low 8bits of OCR1C
}

void timer_3_pwm_A_set	(unsigned char state)
{
	if (state ==2)
	{
		sbi(TCCR3A,COM3A1);
		sbi(TCCR3A,COM3A0);
	}
	else if (state)
	{
		sbi(TCCR3A,COM3A1);
		cbi(TCCR3A,COM3A0);
	}
	else
	{
		cbi(TCCR3A,COM3A1);
		cbi(TCCR3A,COM3A0);
	}
}
void timer_3_pwm_B_set	(unsigned char state)
{
	if (state ==2)
	{
		sbi(TCCR3A,COM3B1);
		sbi(TCCR3A,COM3B0);
	}
	else if (state)
	{
		sbi(TCCR3A,COM3B1);
		cbi(TCCR3A,COM3B0);
	}
	else
	{
		cbi(TCCR3A,COM3B1);
		cbi(TCCR3A,COM3B0);
	}
}
void timer_3_pwm_C_set	(unsigned char state)
{
	if (state ==2)
	{
		sbi(TCCR3A,COM3C1);
		sbi(TCCR3A,COM3C0);
	}
	else if (state)
	{
		sbi(TCCR3A,COM3C1);
		cbi(TCCR3A,COM3C0);
	}
	else
	{
		cbi(TCCR3A,COM3C1);
		cbi(TCCR3A,COM3C0);
	}
}

void timer_3_pwm_A_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(3,nValue);
	outb(OCR3AH, (pwmDuty>>8));		// set the high 8bits of OCR3A
	outb(OCR3AL, (pwmDuty&0x00FF));	// set the low 8bits of OCR3A
}
void timer_3_pwm_B_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(3,nValue);
	outb(OCR3BH, (pwmDuty>>8));		// set the high 8bits of OCR3B
	outb(OCR3BL, (pwmDuty&0x00FF));	// set the low 8bits of OCR3B
}
void timer_3_pwm_C_set_value (float nValue)
{
	unsigned short	pwmDuty = timer_X_pwm_value(3,nValue);
	outb(OCR3CH, (pwmDuty>>8));		// set the high 8bits of OCR3C
	outb(OCR3CL, (pwmDuty&0x00FF));	// set the low 8bits of OCR3C
}


//! Interrupt handler for tcnt0 overflow interrupt
SIGNAL(SIG_OVERFLOW0)
{
	// if a user function is defined, execute it too
	if(timer_int_func[(enum _eTimerInterrupts)eTimer0_Overflow])
		timer_int_func[(enum _eTimerInterrupts)eTimer0_Overflow]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer0_Overflow);
#endif
}

//! Interrupt handler for Timer1 overflow interrupt
SIGNAL(SIG_OVERFLOW1)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer1_Overflow])
		timer_int_func[(enum _eTimerInterrupts)eTimer1_Overflow]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer1_Overflow);
#endif
}

//! Interrupt handler for Timer2 overflow interrupt
SIGNAL(SIG_OVERFLOW2)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer2_Overflow])
		timer_int_func[(enum _eTimerInterrupts)eTimer2_Overflow]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer2_Overflow);
#endif
}

//! Interrupt handler for Timer3 overflow interrupt
SIGNAL(SIG_OVERFLOW3)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer3_Overflow])
		timer_int_func[(enum _eTimerInterrupts)eTimer3_Overflow]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer3_Overflow);
#endif
}

//! Interrupt handler for OutputCompare1A match (OC1A) interrupt
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer1_CompareA])
		timer_int_func[(enum _eTimerInterrupts)eTimer1_CompareA]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer1_CompareA);
#endif
}

//! Interrupt handler for OutputCompare1B match (OC1B) interrupt
SIGNAL(SIG_OUTPUT_COMPARE1B)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer1_CompareB])
		timer_int_func[(enum _eTimerInterrupts)eTimer1_CompareB]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer1_CompareB);
#endif
}

//! Interrupt handler for OutputCompare1C match (OC1C) interrupt
SIGNAL(SIG_OUTPUT_COMPARE1C)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer1_CompareC])
		timer_int_func[(enum _eTimerInterrupts)eTimer1_CompareC]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer1_CompareC);
#endif
}

//! Interrupt handler for InputCapture1(IC1) interrupt
SIGNAL(SIG_INPUT_CAPTURE1)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer1_Capture])
		timer_int_func[(enum _eTimerInterrupts)eTimer1_Capture]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer1_Capture);
#endif
}

//! Interrupt handler for OutputCompare3A match (OC3A) interrupt
SIGNAL(SIG_OUTPUT_COMPARE3A)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer3_CompareA])
		timer_int_func[(enum _eTimerInterrupts)eTimer3_CompareA]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer3_CompareA);
#endif
}

//! Interrupt handler for OutputCompare3B match (OC3B) interrupt
SIGNAL(SIG_OUTPUT_COMPARE3B)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer3_CompareB])
		timer_int_func[(enum _eTimerInterrupts)eTimer3_CompareB]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer3_CompareB);
#endif
}

//! Interrupt handler for OutputCompare3C match (OC3C) interrupt
SIGNAL(SIG_OUTPUT_COMPARE3C)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer3_CompareC])
		timer_int_func[(enum _eTimerInterrupts)eTimer3_CompareC]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer3_CompareC);
#endif
}

//! Interrupt handler for InputCapture3 (IC3) interrupt
SIGNAL(SIG_INPUT_CAPTURE3)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer3_Capture])
		timer_int_func[(enum _eTimerInterrupts)eTimer3_Capture]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer3_Capture);
	#endif
}

#ifdef atmega1280
//! Interrupt handler for Timer3 overflow interrupt
SIGNAL(SIG_OVERFLOW4)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer4_Overflow])
		timer_int_func[(enum _eTimerInterrupts)eTimer4_Overflow]();

#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer4_Overflow);
#endif
}
//! Interrupt handler for Timer3 overflow interrupt
SIGNAL(SIG_OVERFLOW5)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer5_Overflow])
		timer_int_func[(enum _eTimerInterrupts)eTimer5_Overflow]();
#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer5_Overflow);
#endif
}
//! Interrupt handler for OutputCompare0 match (OC0) interrupt
SIGNAL(SIG_OUTPUT_COMPARE0A)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer0_CompareA])
		timer_int_func[(enum _eTimerInterrupts)eTimer0_CompareA]();
#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer0_CompareA);
#endif
}
//! Interrupt handler for OutputCompare0 match (OC0) interrupt
SIGNAL(SIG_OUTPUT_COMPARE0B)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer0_CompareB])
		timer_int_func[(enum _eTimerInterrupts)eTimer0_CompareB]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer0_CompareB);
#endif
}

//! Interrupt handler for OutputCompare2 match (OC2) interrupt
SIGNAL(SIG_OUTPUT_COMPARE2A)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer2_CompareA])
		timer_int_func[(enum _eTimerInterrupts)eTimer2_CompareA]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer2_CompareA);
#endif
}

//! Interrupt handler for OutputCompare2 match (OC2) interrupt
SIGNAL(SIG_OUTPUT_COMPARE2B)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer2_CompareB])
		timer_int_func[(enum _eTimerInterrupts)eTimer2_CompareB]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer2_CompareB);
#endif
}

//! Interrupt handler for OutputCompare4A match (OC4A) interrupt
SIGNAL(SIG_OUTPUT_COMPARE4A)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer4_CompareA])
		timer_int_func[(enum _eTimerInterrupts)eTimer4_CompareA]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer4_CompareA);
#endif
}

//! Interrupt handler for OutputCompare4B match (OC4B) interrupt
SIGNAL(SIG_OUTPUT_COMPARE4B)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer4_CompareB])
		timer_int_func[(enum _eTimerInterrupts)eTimer4_CompareB]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer4_CompareB);
#endif
}

//! Interrupt handler for OutputCompare4C match (OC4C) interrupt
SIGNAL(SIG_OUTPUT_COMPARE4C)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer4_CompareC])
		timer_int_func[(enum _eTimerInterrupts)eTimer4_CompareC]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer4_CompareC);
#endif
}

//! Interrupt handler for InputCapture4 (IC4) interrupt
SIGNAL(SIG_INPUT_CAPTURE4)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer4_Capture])
		timer_int_func[(enum _eTimerInterrupts)eTimer4_Capture]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer4_Capture);
#endif
}

//! Interrupt handler for OutputCompare5A match (OC5A) interrupt
SIGNAL(SIG_OUTPUT_COMPARE5A)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer5_CompareA])
		timer_int_func[(enum _eTimerInterrupts)eTimer5_CompareA]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer5_CompareA);
#endif
}

//! Interrupt handler for OutputCompare5B match (OC5B) interrupt
SIGNAL(SIG_OUTPUT_COMPARE5B)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer5_CompareB])
		timer_int_func[(enum _eTimerInterrupts)eTimer5_CompareB]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer5_CompareB);
#endif
}

//! Interrupt handler for OutputCompare5C match (OC5C) interrupt
SIGNAL(SIG_OUTPUT_COMPARE5C)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer5_CompareC])
		timer_int_func[(enum _eTimerInterrupts)eTimer5_CompareC]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer5_CompareC);
#endif
}

//! Interrupt handler for InputCapture5 (IC5) interrupt
SIGNAL(SIG_INPUT_CAPTURE5)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer5_Capture])
		timer_int_func[(enum _eTimerInterrupts)eTimer5_Capture]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer5_Capture);
#endif
}
#else
//! Interrupt handler for OutputCompare0 match (OC0) interrupt
SIGNAL(SIG_OUTPUT_COMPARE0)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer0_Compare])
		timer_int_func[(enum _eTimerInterrupts)eTimer0_Compare]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer0_Compare);
#endif
}

//! Interrupt handler for OutputCompare2 match (OC2) interrupt
SIGNAL(SIG_OUTPUT_COMPARE2)
{
	// if a user function is defined, execute it
	if(timer_int_func[(enum _eTimerInterrupts)eTimer2_Compare])
		timer_int_func[(enum _eTimerInterrupts)eTimer2_Compare]();
	#ifdef DEBUGTIMER
	uart0SendByte((enum _eTimerInterrupts)eTimer2_Compare);
#endif
}


#endif
